import sim
import numpy as np

#在仿真环境中，机械臂的关节1和关节2的DH参数
#计算实际运动后的位置  以及 理论位置
#最后计算误差

# 连接到 CoppeliaSim 远程 API 服务器
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
    print('Connected to CoppeliaSim')
else:
    print('Failed to connect to CoppeliaSim')
    exit()

# 定义旋转矩阵函数
def rotation_matrix(axis, theta):
    axis = np.asarray(axis)
    axis = axis / np.linalg.norm(axis)
    a = np.cos(theta / 2.0)
    b, c, d = -axis * np.sin(theta / 2.0)
    aa, bb, cc, dd = a * a, b * b, c * c, d * d
    bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d
    rot_mat = np.array([[aa + bb - cc - dd, 2 * (bc + ad), 2 * (bd - ac)],
                        [2 * (bc - ad), aa + cc - bb - dd, 2 * (cd + ab)],
                        [2 * (bd + ac), 2 * (cd - ab), aa + dd - bb - cc]])
    return rot_mat

# 定义关节的DH参数和转动角度
joint1_dh = {
    'alpha': np.deg2rad(-90),  # 关节1的扭转角度
    'a': 0.026,               # 关节1的连杆长度
    'd': 0,                   # 关节1的偏移距离
    'theta': np.deg2rad(30)    # 关节1的转动角度
}
joint2_dh = {
    'alpha': np.deg2rad(0),    # 关节2的扭转角度
    'a': -0.097,                   # 关节2的连杆长度
    'd': 0.23201,              # 关节2的偏移距离
    'theta': np.deg2rad(10)    # 关节2的转动角度
}

# 计算正运动学理论上的位置
def forward_kinematics(joint1_dh, joint2_dh):
    # 计算关节1的变换矩阵
    T01 = np.eye(4)
    T01[:3, :3] = rotation_matrix([0, 1, 0], joint1_dh['theta'])  # 绕y轴旋转
    T01[:3, 3] = [joint1_dh['a'] * np.cos(joint1_dh['alpha']), joint1_dh['a'] * np.sin(joint1_dh['alpha']), joint1_dh['d']]
    
    # 计算关节2的变换矩阵
    T12 = np.eye(4)
    T12[:3, :3] = rotation_matrix([1, 0, 0], joint2_dh['theta'])  # 绕x轴旋转
    T12[:3, 3] = [joint2_dh['a'] * np.cos(joint2_dh['alpha']), joint2_dh['a'] * np.sin(joint2_dh['alpha']), joint2_dh['d']]
    
    # 计算末端执行器的位置
    T02 = np.dot(T01, T12)
    return T02[:3, 3]

# 计算末端执行器的理论位置
end_effector_position_theory = forward_kinematics(joint1_dh, joint2_dh)
print("NumPy array with commas:", ', '.join(map(str, end_effector_position_theory)))

# 获取Dummy当前实际位置
object_name = 'Dummy'
returnCode, handle = sim.simxGetObjectHandle(clientID, object_name, sim.simx_opmode_blocking)
if returnCode == sim.simx_return_ok:
    # 获取物体的位置
    returnCode, position = sim.simxGetObjectPosition(clientID, handle, -1, sim.simx_opmode_blocking)
    if returnCode == sim.simx_return_ok:
        print(f'The current position of {object_name} is: {position}')
    else:
        print('Failed to get the position of the object')
else:
    print('Failed to get the handle of the object')

# 计算误差
error = np.linalg.norm(np.array(end_effector_position_theory) - np.array(position))
print("Position error:", error)

# 关闭连接
sim.simxFinish(clientID)